# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
import inspect
from BipeRobot import BipeRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 

msg = """
Keyboard to control :
1 : Initialize
2 : Go Forward
3 : Go Back
"""

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"

class BipeRobotClass:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "10.10.100.254"
        self.port = 8899
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 导入配置文件，获取文件中的模块ID和模块连接位置
        self.module_ids, self.module_ids_loc = self.robotlib.loadConfig(config_file)
        # 扫描当前机器人构型的ID，默认扫描ID段为20-50
        self.module_ids_scan, self.module_ids_loc_scan = self.mlib.ScanId()
        # 判断是否扫描的ID与配置文件导入的ID一致
        res = BwHelpLib.checkListEqual(self.module_ids,self.module_ids_scan)
        if res == False:
            print("\033[31m输入的模块ID与扫描获取的模块ID不一致，请检查！\033[37m")
            print("配置文件中模块ID: ", self.module_ids)
            print("实际扫描的模块ID: ", self.module_ids_scan)
            exit(0)
        else:
            print("输入的模块ID与扫描获取的模块ID一致")
            
        # 实例化操作臂类
        self.bipeRobot = BipeRobot(self.mlib)
        # 将模块ID与操作臂构型绑定
        self.bipeRobot.bindIds(self.module_ids)        

    #处理键盘
    def handleControlKeys(cmd):
        if(cmd == '1'):
            bipeRobot.initWalkingMode()
        if(cmd == '2'):
            bipeRobot.goForward_bipe()
        if(cmd == '3'):
            bipeRobot.goBack_bipe()

if __name__ == "__main__":
    try :
        bipe_robot = BipeRobotClass()    
        print(msg)
        while(1):
            key = BwHelpLib.readkey()
            bipe_robot.handleControlKeys(key)     
            if (key == '\x03'): # Ctrl+C 退出程序
                break        
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass

